#include "mainwindow.h"
#include "./ui_mainwindow.h"
#include "qss_style.h"
#include <QDebug>
#include <QGraphicsRectItem>
#include <QGraphicsLineItem>
#include <QPen>
#include <QBrush>
#include <QPainter>
#include <QGraphicsDropShadowEffect>

AnimationWidget::AnimationWidget(QWidget *parent, MainWindow * mainWindow) :
    QWidget(parent), mainWindow(mainWindow) {

    // 设置垃圾车位置
    // truckPos = QPoint(100, 300);
    truckPos = QPoint(TRUCK_POS_X, TRUCK_POS_Y);

    // 加载机器人、小车和垃圾桶的贴图
    QPixmap robotOriginPixmap;
    robotOriginPixmap.load(":/pngs/icons_robot_30.png");
    robotPixmap = robotOriginPixmap.scaled(
        ROBOT_SCALED_SIZE,
        ROBOT_SCALED_SIZE,
        Qt::IgnoreAspectRatio,
        Qt::SmoothTransformation
    );

    QPixmap robotWorkOriginPixmap;
    robotWorkOriginPixmap.load(":/pngs/icons_robotwork.png");
    // robotWorkPixmap.load(":/pngs/icons_robotwork.png");
    robotWorkPixmap = robotWorkOriginPixmap.scaled(
        ROBOT_SCALED_SIZE,
        ROBOT_SCALED_SIZE,
        Qt::IgnoreAspectRatio,
        Qt::SmoothTransformation
    );

    // truckPixmap.load(":/pngs/icons_truck_96.png");
    // trashPixmap.load(":/pngs/icons_trash.png"); // 假设垃圾桶贴图路径
    QPixmap trashOriginPixmap;
    trashOriginPixmap.load(":/pngs/icons_trash.png");
    trashPixmap = trashOriginPixmap.scaled(
        TRASH_SCALED_SIZE,
        TRASH_SCALED_SIZE,
        Qt::IgnoreAspectRatio,
        Qt::SmoothTransformation
    );

    QPixmap truckOriginPixmap;
    truckOriginPixmap.load(":/pngs/icons_truck_96.png");
    truckPixmap = truckOriginPixmap.scaled(
        TRUCK_SCALED_SIZE,
        TRUCK_SCALED_SIZE,
        Qt::IgnoreAspectRatio,
        Qt::SmoothTransformation
    );

    if (robotPixmap.isNull() || truckPixmap.isNull() || trashPixmap.isNull()) {
        qDebug() << "Failed to load pixmaps!";
    }

    t1 = 0.0;
    t2 = 0.0;
    t3 = 0.0;
    passedMiddlePoint = false;
    robotReachedTruck = false;
    currentStage = 0;

    updateTimer = new QTimer(this);
    connect(updateTimer, &QTimer::timeout, this, &AnimationWidget::updatePositions);
    updateTimer->start(16); // 约 60 FPS
}

AnimationWidget::~AnimationWidget() {
    delete updateTimer;
}

void AnimationWidget::paintEvent(QPaintEvent *event) {
    Q_UNUSED(event);
    QPainter painter(this);

    // 绘制曲线，设置路径画笔
    // QPen pathPen(Qt::blue);
    // pathPen.setWidth(4);
    // pathPen.setCapStyle(Qt::RoundCap);
    // pathPen.setJoinStyle(Qt::RoundJoin);
    // painter.setPen(pathPen);
    // painter.drawPath(chargingRoomToTrashPath);
    // painter.drawPath(trashToTruckPath);
    // painter.drawPath(truckToTrashPath);
    // painter.drawPath(trashToChargingPath);

    // 绘制控制点和控制线（帮助理解贝塞尔曲线的形成）
    // QPen controlPen(Qt::red);
    // controlPen.setStyle(Qt::DashLine);
    // controlPen.setWidth(1);
    // painter.setPen(controlPen);

    // // 绘制起点到第一个控制点的辅助线
    // painter.drawLine(chargingToTrashPathStartP, chargingToTrashPathCtrlP);
    // // 绘制第二个控制点到终点的辅助线
    // painter.drawLine(chargingToTrashPathCtrlP, chargingToTrashPathEndP);
    // // 绘制起点到第一个控制点的辅助线
    // painter.drawLine(trashToTruckPathStartP, trashToTruckPathCtrlP);
    // // 绘制第二个控制点到终点的辅助线
    // painter.drawLine(trashToTruckPathCtrlP, trashToTruckPathEndP);

    // 绘制边框
    QPen pen(Qt::black, 20); // 黑色边框，宽度为 8
    painter.setPen(pen);
    painter.drawRect(rect());

    // 绘制机器人贴图
    if (!passedMiddlePoint) {
        painter.drawPixmap(robotPos.x() - robotPixmap.width() / 2, robotPos.y() - robotPixmap.height() / 2, robotPixmap);
    } else if(!robotReachedTruck){
        painter.drawPixmap(robotPos.x() - robotWorkPixmap.width() / 2, robotPos.y() - robotWorkPixmap.height() / 2, robotWorkPixmap);
    }else{
        painter.drawPixmap(robotPos.x() - robotPixmap.width() / 2, robotPos.y() - robotPixmap.height() / 2, robotPixmap);
    }

    // 绘制小车贴图
    painter.drawPixmap(truckPos.x() - truckPixmap.width() / 2, truckPos.y() - truckPixmap.height() / 2, truckPixmap);

    // 设置画笔属性，包括颜色和粗细
    pen.setWidth(5); // 设置边框粗细为 5 像素
    QBrush brush(Qt::lightGray);
    brush.setStyle(Qt::SolidPattern);
    // 设置矩形透明（通过设置画刷透明度）
    brush.setColor(QColor(211, 211, 211, 128)); // lightGray 半透明（alpha=128）
    painter.setBrush(brush);
    painter.setPen(pen);

    // painter.fillRect(chargingRoomRect, Qt::lightGray);
    // painter.setPen(pen);
    painter.drawRect(chargingRoomRect);

    // 在矩形中添加文字
    QFont font(" Microsoft YaHei", 20, QFont::Bold); // 使用构造函数设置字体家族、大小和粗细
    painter.setFont(font);
    QRect textRect = chargingRoomRect.adjusted(0, -170, 0, 0);
    painter.drawText(textRect, Qt::AlignCenter, "充电间");

    // 绘制3个垃圾桶
    painter.drawPixmap(trashPos1.x() - trashPixmap.width() / 2 + 20, trashPos1.y() - trashPixmap.height() / 2, trashPixmap);
    painter.drawPixmap(trashPos2.x() - trashPixmap.width() / 2, trashPos2.y() - trashPixmap.height() / 2, trashPixmap);
    painter.drawPixmap(trashPos3.x() - trashPixmap.width() / 2 - 20, trashPos3.y() - trashPixmap.height() / 2, trashPixmap);
}

void AnimationWidget::resizeEvent(QResizeEvent *event) {
    QWidget::resizeEvent(event);
    chargingRoomRect.setX(width() - chargingRoomRectWidth - chargingRoomRectMargin);
    chargingRoomRect.setY(chargingRoomRectMargin);
    chargingRoomRect.setWidth(chargingRoomRectWidth);
    chargingRoomRect.setHeight(chargingRoomRectHeight);

    // qInfo("Animation widget height:%d,width:%d\n",height(), width());

    // 设置机器人运动起始点(机器人会运动到原点,起始点也是运动终点)
    startPoint = QPoint(chargingRoomRect.x() + chargingRoomRectWidth / 2,
                        chargingRoomRect.y() + chargingRoomRectHeight / 2);
    // 设置机器人运动目标点
    targetPoint = QPoint(truckPos.x() + 10, truckPos.y() + 100);
    // 设置机器人运动中间点
    middlePoint = QPoint((startPoint.x() + targetPoint.x()) / 2, (startPoint.y() + targetPoint.y()) / 2);

    // 设置机器人起始位置为运动起始点
    robotPos = startPoint;
    // 设置垃圾桶初始位置
    trashPos1 = QPoint(middlePoint.x() + 50, middlePoint.y());
    trashPos2 = QPoint(middlePoint.x(), middlePoint.y());
    trashPos3 = QPoint(middlePoint.x() - 50, middlePoint.y());

    // 充电间至垃圾桶贝塞尔曲线的起始点与终点
    chargingToTrashPathStartP = robotPos;
    chargingToTrashPathEndP = QPoint(middlePoint.x() + 100, middlePoint.y());
    chargingToTrashPathEndP.setX(chargingToTrashPathEndP.x() + 25);
    // 充电间至垃圾桶贝塞尔曲线的控制点
    chargingToTrashPathCtrlP = QPoint(chargingToTrashPathStartP.x() + 40, chargingToTrashPathStartP.y() + 150);
    // 充电间至垃圾桶贝塞尔曲线生成
    chargingRoomToTrashPath.moveTo(chargingToTrashPathStartP);
    chargingRoomToTrashPath.quadTo(chargingToTrashPathCtrlP, chargingToTrashPathEndP);

    qDebug() << "trashPos1.x:" << trashPos1.x()<< ", chargingToTrashPathEndP.x:" << chargingToTrashPathEndP.x();

    // 垃圾桶初始位置至垃圾车贝塞尔曲线的起始点与终点
    // trashToTruckPathStartP = QPoint(middlePoint.x() + 80, middlePoint.y());
    trashToTruckPathStartP = QPoint(chargingToTrashPathEndP.x(), chargingToTrashPathEndP.y());
    // trashToTruckPathStartP.setX(trashToTruckPathStartP.x() + 28);
    trashToTruckPathEndP = targetPoint;
    // 垃圾桶初始位置至垃圾车贝塞尔曲线的控制点
    trashToTruckPathCtrlP = QPoint(trashToTruckPathStartP.x() + 30, trashToTruckPathEndP.y() + 40);
    // 垃圾桶初始位置至垃圾车贝塞尔曲线生成
    trashToTruckPath.moveTo(trashToTruckPathStartP);
    trashToTruckPath.quadTo(trashToTruckPathCtrlP, trashToTruckPathEndP);
    
    // 从垃圾车返回垃圾桶初始位置的贝塞尔曲线的起始点与终点
    truckToTrashPathStartP = trashToTruckPathEndP;
    // truckToTrashPathEndP = trashToTruckPathStartP;
    // truckToTrashPathEndP.setX(trashToTruckPathStartP.x());
    // truckToTrashPathEndP.setY(trashToTruckPathStartP.y());
    truckToTrashPathEndP = chargingToTrashPathEndP;
    truckToTrashPathCtrlP = trashToTruckPathCtrlP;
    truckToTrashPath.moveTo(truckToTrashPathStartP);
    truckToTrashPath.quadTo(truckToTrashPathCtrlP, truckToTrashPathEndP);

    // 返回充电间贝塞尔曲线的起始点与终点
    trashToChargingPathStartP = truckToTrashPathEndP;
    // trashToChargingPathEndP = QPoint(middlePoint.x() - 50, middlePoint.y());;
    trashToChargingPathEndP = startPoint;
    // 返回充电间贝塞尔曲线的控制点
    trashToChargingPathCtrlP = chargingToTrashPathCtrlP;
    // 返回充电间贝塞尔曲线生成
    trashToChargingPath.moveTo(trashToChargingPathStartP);
    trashToChargingPath.quadTo(trashToChargingPathCtrlP, trashToChargingPathEndP);
}

void AnimationWidget::updatePosition(const QPainterPath &path, double &t, double targetT, std::function<void ()> callback)
{
    if (t >= targetT) {
        callback();
    } else {
        QPointF newPos = path.pointAtPercent(t);
        robotPos = newPos.toPoint();
        t += 0.005;
    }
}

void AnimationWidget::updatePositions() {
    if (currentStage == 1) {
        updatePosition(chargingRoomToTrashPath, t1, 1.0, [this]() {
            // 完成第一阶段移动，机器人从充电间至垃圾桶
        });
    } else if (currentStage == 2) {
        updatePosition(trashToTruckPath, t2, 1.0, [this]() {
            robotReachedTruck = true;
        });
        trashPos1 = QPoint(robotPos.x() - 75, robotPos.y());
    } else if (currentStage == 3) {
        updatePosition(truckToTrashPath, t3, 1.0, [this]()
        {  });
        trashPos1 = QPoint(robotPos.x() - 75, robotPos.y());
    } else if(currentStage == 4) {
        updatePosition(trashToChargingPath, t4, 1.0, [this]()
        {
            updateTimer->stop();
        });
    } 

    update();
}

void AnimationWidget::processStep1()
{
    if (currentStage == 0) {
        currentStage = 1;
        t1 = 0.0;
    }

    if(mainWindow)
    {
        mainWindow->setLabelStepGreen(1);
        mainWindow->setLabelArrowGreen(1);

        mainWindow->setLabelStepGrey(2);
        mainWindow->setLabelArrowGrey(2);
        mainWindow->setLabelStepGrey(3);
    }
}

void AnimationWidget::processStep2()
{
    if (currentStage == 1 && t1 >= 1.0) {
        passedMiddlePoint = true;
    }

    if(mainWindow)
    {
        mainWindow->setLabelStepGreen(2);
        mainWindow->setLabelArrowGreen(2);

        mainWindow->setLabelStepGrey(1);
        mainWindow->setLabelArrowGrey(1);
        mainWindow->setLabelStepGrey(3);
    }
}

void AnimationWidget::processStep3()
{
    if (currentStage == 1 && t1 >= 1.0) {
        currentStage = 2;
        t2 = 0.0;
    }

    if(mainWindow)
    {
        mainWindow->setLabelStepGreen(3);
        mainWindow->setLabelStepGrey(1);
        mainWindow->setLabelArrowGrey(1);
        mainWindow->setLabelStepGrey(2);
        mainWindow->setLabelArrowGrey(2);
    }
}

void AnimationWidget::processStep4()
{
    if (currentStage == 2 && t2 >= 1.0) {
        currentStage = 3;
        t3 = 0.0;
    }

    if(mainWindow)
    {
        mainWindow->setLabelStepGrey(1);
        mainWindow->setLabelArrowGrey(1);
        mainWindow->setLabelStepGrey(2);
        mainWindow->setLabelArrowGrey(2);
        mainWindow->setLabelStepGrey(3);
    }
}

void AnimationWidget::processStep5()
{
    if (currentStage == 3 && t3 >= 1.0) {
        currentStage = 4;
        t4 = 0.0;
    }

    if(mainWindow)
    {
        mainWindow->setLabelStepGrey(1);
        mainWindow->setLabelArrowGrey(1);
        mainWindow->setLabelStepGrey(2);
        mainWindow->setLabelArrowGrey(2);
        mainWindow->setLabelStepGrey(3);
    }
}

void AnimationWidget::processStep6()
{
    // 重置动画相关状态变量
    t1 = 0.0;
    t2 = 0.0;
    t3 = 0.0;
    t4 = 0.0;
    passedMiddlePoint = false;
    robotReachedTruck = false;
    // carMovingToChargingRoom = false;
    currentStage = 0;
    // 重置机器人位置
    robotPos = startPoint;
    // // 重置垃圾桶位置
    // trashPos1 = middlePoint;
    // 重启定时器
    if (!updateTimer->isActive()) {
        updateTimer->start(16);
    }

    if(mainWindow)
    {
        mainWindow->setLabelStepGrey(1);
        mainWindow->setLabelArrowGrey(1);
        mainWindow->setLabelStepGrey(2);
        mainWindow->setLabelArrowGrey(2);
        mainWindow->setLabelStepGrey(3);
    }
}

void MainWindow::widgetsInit()
{
    mainLayout = new QGridLayout(ui->centralwidget);
    autoModeLayout = new QGridLayout();
    autoModeWidget = new QWidget();
    // auto page widgets init
    modeStatusLayout = new QGridLayout();
    modeStatusWidget = new QWidget();
    ctrlBtnsLayout = new QHBoxLayout();
    ctrlBtnsWidget = new QWidget();
    taskStatusLayout = new QVBoxLayout();
    taskStatusWidget = new QWidget();
    animationWidget = new AnimationWidget(this, this);
    
    // manual page widgets init
    posDisplayLayout = new QGridLayout();
    posDisplayWidget = new QWidget();
    pos_x_layout = new QHBoxLayout();
    pos_x_widget = new QWidget();
    pos_y_layout = new QHBoxLayout();
    pos_y_widget = new QWidget();
    chCtrlLayout = new QGridLayout();
    chCtrlWidget = new QWidget();
    armCtrlLayout = new QGridLayout();
    armCtrlWidget = new QWidget();
    leftArmRadioButton = new QRadioButton();
    rightArmRadioButton = new QRadioButton();
    robotEnPushbutton = new QPushButton;
    pushButton_X_plus = new QPushButton();
    pushButton_X_minus = new QPushButton();
    pushButton_Y_plus = new QPushButton();
    pushButton_Y_minus = new QPushButton();
    pushButton_Z_plus = new QPushButton();
    pushButton_Z_minus = new QPushButton();
    pushButton_Rx_plus = new QPushButton();
    pushButton_Rx_minus = new QPushButton();
    pushButton_Ry_plus = new QPushButton();
    pushButton_Ry_minus = new QPushButton();
    pushButton_Rz_plus = new QPushButton();
    pushButton_Rz_minus = new QPushButton();

    label_joint = new QLabel();
    label_joint_L = new QLabel();
    label_joint_R = new QLabel();
    label_L_arm_J1 = new QLabel();
    label_L_arm_J2 = new QLabel();
    label_L_arm_J3 = new QLabel();
    label_L_arm_J4 = new QLabel();
    label_L_arm_J5 = new QLabel();
    label_L_arm_J6 = new QLabel();
    label_L_arm_J7 = new QLabel();
    lineEdit_L_arm_J1 = new QLineEdit();
    lineEdit_L_arm_J2 = new QLineEdit();
    lineEdit_L_arm_J3 = new QLineEdit();
    lineEdit_L_arm_J4 = new QLineEdit();
    lineEdit_L_arm_J5 = new QLineEdit();
    lineEdit_L_arm_J6 = new QLineEdit();
    lineEdit_L_arm_J7 = new QLineEdit();
    L_arm_J1_layout = new QHBoxLayout();
    L_arm_J1_widget = new QWidget();
    L_arm_J2_layout = new QHBoxLayout();
    L_arm_J2_widget = new QWidget();
    L_arm_J3_layout = new QHBoxLayout();
    L_arm_J3_widget = new QWidget();
    L_arm_J4_layout = new QHBoxLayout();
    L_arm_J4_widget = new QWidget();
    L_arm_J5_layout = new QHBoxLayout();
    L_arm_J5_widget = new QWidget();
    L_arm_J6_layout = new QHBoxLayout();
    L_arm_J6_widget = new QWidget();
    L_arm_J7_layout = new QHBoxLayout();
    L_arm_J7_widget = new QWidget();

    label_R_arm_J1 = new QLabel();
    label_R_arm_J2 = new QLabel();
    label_R_arm_J3 = new QLabel();
    label_R_arm_J4 = new QLabel();
    label_R_arm_J5 = new QLabel();
    label_R_arm_J6 = new QLabel();
    label_R_arm_J7 = new QLabel();
    lineEdit_R_arm_J1 = new QLineEdit();
    lineEdit_R_arm_J2 = new QLineEdit();
    lineEdit_R_arm_J3 = new QLineEdit();
    lineEdit_R_arm_J4 = new QLineEdit();
    lineEdit_R_arm_J5 = new QLineEdit();
    lineEdit_R_arm_J6 = new QLineEdit();
    lineEdit_R_arm_J7 = new QLineEdit();
    R_arm_J1_layout = new QHBoxLayout();
    R_arm_J1_widget = new QWidget();
    R_arm_J2_layout = new QHBoxLayout();
    R_arm_J2_widget = new QWidget();
    R_arm_J3_layout = new QHBoxLayout();
    R_arm_J3_widget = new QWidget();
    R_arm_J4_layout = new QHBoxLayout();
    R_arm_J4_widget = new QWidget();
    R_arm_J5_layout = new QHBoxLayout();
    R_arm_J5_widget = new QWidget();
    R_arm_J6_layout = new QHBoxLayout();
    R_arm_J6_widget = new QWidget();
    R_arm_J7_layout = new QHBoxLayout();
    R_arm_J7_widget = new QWidget();
    armJointLayout = new QGridLayout();
    armJointWidget = new QWidget();

    label_coord_L = new QLabel();
    label_coord_R = new QLabel();
    label_L_arm_X = new QLabel();
    label_L_arm_Y = new QLabel();
    label_L_arm_Z = new QLabel();
    label_L_arm_Rx = new QLabel();
    label_L_arm_Ry = new QLabel();
    label_L_arm_Rz = new QLabel();
    lineEdit_L_arm_X = new QLineEdit();
    lineEdit_L_arm_Y = new QLineEdit();
    lineEdit_L_arm_Z = new QLineEdit();
    lineEdit_L_arm_Rx = new QLineEdit();
    lineEdit_L_arm_Ry = new QLineEdit();
    lineEdit_L_arm_Rz = new QLineEdit();
    L_arm_X_layout = new QHBoxLayout();
    L_arm_X_widget = new QWidget();
    L_arm_Y_layout = new QHBoxLayout();
    L_arm_Y_widget = new QWidget();
    L_arm_Z_layout = new QHBoxLayout();
    L_arm_Z_widget = new QWidget();
    L_arm_Rx_layout = new QHBoxLayout();
    L_arm_Rx_widget = new QWidget();
    L_arm_Ry_layout = new QHBoxLayout();
    L_arm_Ry_widget = new QWidget();
    L_arm_Rz_layout = new QHBoxLayout();
    L_arm_Rz_widget = new QWidget();
    label_R_arm_X = new QLabel();
    label_R_arm_Y = new QLabel();
    label_R_arm_Z = new QLabel();
    label_R_arm_Rx = new QLabel();
    label_R_arm_Ry = new QLabel();
    label_R_arm_Rz = new QLabel();
    lineEdit_R_arm_X = new QLineEdit();
    lineEdit_R_arm_Y = new QLineEdit();
    lineEdit_R_arm_Z = new QLineEdit();
    lineEdit_R_arm_Rx = new QLineEdit();
    lineEdit_R_arm_Ry = new QLineEdit();
    lineEdit_R_arm_Rz = new QLineEdit();
    R_arm_X_layout = new QHBoxLayout();
    R_arm_X_widget = new QWidget();
    R_arm_Y_layout = new QHBoxLayout();
    R_arm_Y_widget = new QWidget();
    R_arm_Z_layout = new QHBoxLayout();
    R_arm_Z_widget = new QWidget();
    R_arm_Rx_layout = new QHBoxLayout();
    R_arm_Rx_widget = new QWidget();
    R_arm_Ry_layout = new QHBoxLayout();
    R_arm_Ry_widget = new QWidget();
    R_arm_Rz_layout = new QHBoxLayout();
    R_arm_Rz_widget = new QWidget();
    armCoordLayout = new QGridLayout();
    armCoordWidget = new QWidget();
    armHBoxLayout = new QHBoxLayout();
    armHBoxWidget = new QWidget();

    longPressTimer = new QTimer();
}

void MainWindow::autoModeStatusSet()
{
    ui->label_mode->setAlignment(Qt::AlignRight);
    ui->label_mode->setStyleSheet(label_mode_style);

    ui->pushButton_auto->setStyleSheet(pushButton_auto_style);
    // ui->pushButton_auto->setFixedWidth(120);
    // ui->pushButton_auto->setFixedHeight(74);
    ui->pushButton_auto->setFixedWidth(PUSHBUTTON_AUTO_WIDTH);
    ui->pushButton_auto->setFixedHeight(PUSHBUTTON_AUTO_HEIGHT);

    ui->label_status->setAlignment(Qt::AlignRight);
    ui->label_status->setStyleSheet(label_status_style);

    ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_stop_red_100.png"));
    ui->label_statusIcon->setAlignment(Qt::AlignCenter);
    // ui->label_statusIcon->setFixedSize(80, 80);
    ui->label_statusIcon->setFixedSize(AUTO_ICON_SIZE, AUTO_ICON_SIZE);
    ui->label_statusIcon->setScaledContents(true);

    modeStatusLayout->setHorizontalSpacing(50);
    modeStatusLayout->setVerticalSpacing(20);
    modeStatusLayout->addWidget(ui->label_mode, 0, 0, Qt::AlignCenter);
    modeStatusLayout->addWidget(ui->pushButton_auto, 1, 0, Qt::AlignCenter);
    modeStatusLayout->addWidget(ui->label_status, 0, 1, Qt::AlignCenter);
    modeStatusLayout->addWidget(ui->label_statusIcon, 1, 1, Qt::AlignCenter);
    modeStatusWidget->setLayout(modeStatusLayout);
}

void MainWindow::autoModeCtrlBtnsSet()
{
    ui->pushButton_start->setStyleSheet(pushButton_start_style);
    // ui->pushButton_start->setFixedWidth(120);
    // ui->pushButton_start->setFixedHeight(74);
    ui->pushButton_start->setFixedWidth(PUSHBUTTON_AUTO_WIDTH);
    ui->pushButton_start->setFixedHeight(PUSHBUTTON_AUTO_HEIGHT);

    ui->pushButton_stop->setStyleSheet(pushButton_stop_style);
    // ui->pushButton_stop->setFixedWidth(120);
    // ui->pushButton_stop->setFixedHeight(74);
    ui->pushButton_stop->setFixedWidth(PUSHBUTTON_AUTO_WIDTH);
    ui->pushButton_stop->setFixedHeight(PUSHBUTTON_AUTO_HEIGHT);

    ui->pushButton_pause->setStyleSheet(pushButton_pause_style);
    // ui->pushButton_pause->setFixedWidth(120);
    // ui->pushButton_pause->setFixedHeight(74);
    ui->pushButton_pause->setFixedWidth(PUSHBUTTON_AUTO_WIDTH);
    ui->pushButton_pause->setFixedHeight(PUSHBUTTON_AUTO_HEIGHT);

    // ctrlBtnsLayout->setSpacing(45);
    ctrlBtnsLayout->setSpacing(80);
    ctrlBtnsLayout->addWidget(ui->pushButton_start, Qt::AlignRight);
    ctrlBtnsLayout->addWidget(ui->pushButton_stop, Qt::AlignRight);
    ctrlBtnsLayout->addWidget(ui->pushButton_pause, Qt::AlignRight);   
    ctrlBtnsWidget->setLayout(ctrlBtnsLayout);
}

void MainWindow::autoModeTaskSet()
{
    ui->label_task->setAlignment(Qt::AlignCenter);
    ui->label_task->setStyleSheet("font-family: Microsoft YaHei; font:bold; font-size: 45px;");
    ui->label_task->setFixedWidth(AUTO_LABEL_WIDTH);
    ui->label_task->setFixedHeight(100);

    ui->label_step1->setAlignment(Qt::AlignCenter);
    ui->label_step1->setStyleSheet(label_step_style);
    ui->label_step1->setFixedWidth(AUTO_LABEL_WIDTH);
    ui->label_step1->setFixedHeight(AUTO_LABEL_HEIGHT);

    ui->label_arrow1->setAlignment(Qt::AlignCenter);
    ui->label_arrow1->setPixmap(QPixmap(":/pngs/icons_arrow_grey_90.png"));
    ui->label_arrow1->setFixedWidth(AUTO_ARROW_WIDTH);

    ui->label_step2->setAlignment(Qt::AlignCenter);
    ui->label_step2->setStyleSheet(label_step_style);
    ui->label_step2->setFixedWidth(AUTO_LABEL_WIDTH);
    ui->label_step2->setFixedHeight(AUTO_LABEL_HEIGHT);

    ui->label_arrow2->setAlignment(Qt::AlignCenter);
    ui->label_arrow2->setPixmap(QPixmap(":/pngs/icons_arrow_grey_90.png"));
    ui->label_arrow2->setFixedWidth(AUTO_ARROW_WIDTH);

    ui->label_step3->setAlignment(Qt::AlignCenter);
    ui->label_step3->setStyleSheet(label_step_style);
    ui->label_step3->setFixedWidth(AUTO_LABEL_WIDTH);
    ui->label_step3->setFixedHeight(AUTO_LABEL_HEIGHT);

    taskStatusLayout->addWidget(ui->label_task, Qt::AlignCenter);
    taskStatusLayout->addWidget(ui->label_step1, Qt::AlignHCenter);
    taskStatusLayout->addWidget(ui->label_arrow1, Qt::AlignCenter);
    taskStatusLayout->addWidget(ui->label_step2, Qt::AlignCenter);
    taskStatusLayout->addWidget(ui->label_arrow2, Qt::AlignCenter);
    taskStatusLayout->addWidget(ui->label_step3, Qt::AlignCenter);
    taskStatusWidget->setLayout(taskStatusLayout);
}

void MainWindow::autoModeAnimationSet()
{
    QSizePolicy animation_size_policy(QSizePolicy::Fixed, QSizePolicy::Fixed);
    animationWidget->setSizePolicy(animation_size_policy);
    // animationWidget->setFixedSize(700, 460);
    animationWidget->setFixedSize(ANIMATION_WIDTH, ANIMATION_HEIGHT);
}

void MainWindow::autoModePageHide()
{
    ui->pushButton_auto->setText("手动");
    ui->pushButton_auto->setStyleSheet(pushButton_man_style);

    ui->label_status->hide();
    ui->label_statusIcon->hide();
    ui->pushButton_start->hide();
    ui->pushButton_stop->hide();
    ui->pushButton_pause->hide();
    ui->label_task->hide();
    ui->label_step1->hide();
    ui->label_step2->hide();
    ui->label_step3->hide();
    ui->label_arrow1->hide();
    ui->label_arrow2->hide();
    animationWidget->hide();
}

void MainWindow::autoModePageShow()
{
    ui->label_status->setStyleSheet(label_status_style);
    ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_stop_red_100.png"));
    ui->pushButton_auto->setText("自动");
    ui->pushButton_auto->setStyleSheet(pushButton_auto_style);
    ui->pushButton_start->show();
    ui->pushButton_stop->show();
    ui->pushButton_pause->show();
    ui->label_status->show();
    ui->label_statusIcon->show();
    ui->label_task->show();
    ui->label_step1->show();
    ui->label_step2->show();
    ui->label_step3->show();
    ui->label_arrow1->show();
    ui->label_arrow2->show();
    animationWidget->show();
}

void MainWindow::autoModeLayoutSet()
{
    mainLayout->removeWidget(posDisplayWidget);
    mainLayout->removeWidget(armHBoxWidget);
    mainLayout->removeWidget(armCtrlWidget);
    mainLayout->removeWidget(chCtrlWidget);
    mainLayout->addWidget(modeStatusWidget, 0, 0, 2, 3, Qt::AlignCenter);
    mainLayout->addWidget(ctrlBtnsWidget, 0, 3, 2, 4, Qt::AlignRight);
    mainLayout->addWidget(taskStatusWidget, 2, 0, 6, 3, Qt::AlignCenter);
    mainLayout->addWidget(animationWidget, 2, 3, 6, 4, Qt::AlignCenter);
    mainLayout->setContentsMargins(0, 50, 170, 100);
    mainLayout->update();
    // printLayoutInfo();
}

void MainWindow::manualModePageHide()
{
    ui->label_pos->hide();
    ui->label_pos_X->hide();
    ui->label_pos_Y->hide();

    ui->lineEdit_pos_X->hide();
    ui->lineEdit_pos_Y->hide();

    robotEnPushbutton->hide();

    ui->pushButton_ch_F->hide();
    ui->pushButton_ch_B->hide();
    ui->pushButton_ch_L->hide();
    ui->pushButton_ch_R->hide();

    ui->pushButton_grip_L->hide();
    ui->pushButton_grip_R->hide();

    ui->label_arm->hide();
    ui->label_ch->hide();
    ui->label_gripper->hide();
       
    leftArmRadioButton->hide();
    rightArmRadioButton->hide();

    pushButton_X_plus->hide();
    pushButton_X_minus->hide();

    pushButton_Y_plus->hide();
    pushButton_Y_minus->hide();

    pushButton_Z_plus->hide();
    pushButton_Z_minus->hide();

    pushButton_Rx_plus->hide();
    pushButton_Rx_minus->hide();

    pushButton_Ry_plus->hide();
    pushButton_Ry_minus->hide();

    pushButton_Rz_plus->hide();
    pushButton_Rz_minus->hide();

    posDisplayWidget->hide();
    armJointWidget->hide();
    armCoordWidget->hide();
}

void MainWindow::manualModePageShow()
{
    leftArmRadioButton->show();
    rightArmRadioButton->show();

    pushButton_X_plus->show();
    pushButton_X_minus->show();

    pushButton_Y_plus->show();
    pushButton_Y_minus->show();

    pushButton_Z_plus->show();
    pushButton_Z_minus->show();

    pushButton_Rx_plus->show();
    pushButton_Rx_minus->show();

    pushButton_Ry_plus->show();
    pushButton_Ry_minus->show();

    pushButton_Rz_plus->show();
    pushButton_Rz_minus->show();

    ui->label_pos->show();
    ui->label_pos_X->show();
    ui->label_pos_Y->show();
    ui->lineEdit_pos_X->show();
    ui->lineEdit_pos_Y->show();

    robotEnPushbutton->show();

    ui->pushButton_ch_F->show();
    ui->pushButton_ch_B->show();
    ui->pushButton_ch_L->show();
    ui->pushButton_ch_R->show();

    ui->pushButton_grip_L->show();
    ui->pushButton_grip_R->show();

    ui->label_arm->show();
    ui->label_ch->show();
    ui->label_gripper->show();

    posDisplayWidget->show();
    armJointWidget->show();
    armCoordWidget->show();
}

void MainWindow::manualModePosSet()
{
    ui->label_pos_X->setText("X");
    ui->label_pos_Y->setText("Y");
    ui->label_pos_X->setStyleSheet(label_pos_style);
    ui->label_pos_Y->setStyleSheet(label_pos_style);
    ui->label_pos->setStyleSheet(label_joint_style);

    ui->label_pos_X->setAlignment(Qt::AlignCenter);
    // ui->label_pos_X->setFixedHeight(40);
    // ui->label_pos_X->setFixedWidth(50);
    ui->label_pos_X->setFixedHeight(LABEL_JOINT_HEIGHT);
    ui->label_pos_X->setFixedWidth(LABEL_JOINT_WIDTH);
    ui->label_pos_Y->setAlignment(Qt::AlignCenter);
    // ui->label_pos_Y->setFixedHeight(40);
    // ui->label_pos_Y->setFixedWidth(50); 
    ui->label_pos_Y->setFixedHeight(LABEL_JOINT_HEIGHT);
    ui->label_pos_Y->setFixedWidth(LABEL_JOINT_WIDTH); 

    // ui->lineEdit_pos_X->setFixedHeight(40);
    // ui->lineEdit_pos_X->setFixedWidth(80);
    // ui->lineEdit_pos_Y->setFixedHeight(40);
    // ui->lineEdit_pos_Y->setFixedWidth(80);
    ui->lineEdit_pos_X->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    ui->lineEdit_pos_X->setFixedWidth(LINEEDIT_JOINT_WIDTH);
    ui->lineEdit_pos_Y->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    ui->lineEdit_pos_Y->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    ui->lineEdit_pos_X->setStyleSheet(lineEdit_pos_style);
    ui->lineEdit_pos_Y->setStyleSheet(lineEdit_pos_style);

    pos_x_layout->setSpacing(0);
    pos_x_layout->addWidget(ui->label_pos_X);
    pos_x_layout->addWidget(ui->lineEdit_pos_X);
    pos_x_layout->setSpacing(0);
    pos_x_widget->setLayout(pos_x_layout);

    pos_y_layout->setSpacing(0);
    pos_y_layout->addWidget(ui->label_pos_Y);
    pos_y_layout->addWidget(ui->lineEdit_pos_Y);
    pos_y_layout->setSpacing(0);
    pos_y_widget->setLayout(pos_y_layout);

    posDisplayLayout->addWidget(ui->label_pos, 0, 0, 2, 1, Qt::AlignCenter);
    posDisplayLayout->addWidget(pos_x_widget, 0, 1, 1, 1, Qt::AlignCenter);
    posDisplayLayout->addWidget(pos_y_widget, 1, 1, 1, 1, Qt::AlignCenter);
    posDisplayLayout->setColumnMinimumWidth(0, 200);
    posDisplayWidget->setLayout(posDisplayLayout);
    posDisplayWidget->setObjectName("posDisplayWidget");
    posDisplayWidget->setStyleSheet("#posDisplayWidget { background-color: white; border: 5px solid black; border-radius: 5px; }");
}

void MainWindow::manualGripperSet()
{
    // ui->pushButton_grip_L->setFixedWidth(70);
    // ui->pushButton_grip_L->setFixedHeight(70);
    // ui->pushButton_grip_R->setFixedWidth(70);
    // ui->pushButton_grip_R->setFixedHeight(70);
    ui->pushButton_grip_L->setFixedWidth(GRIPPER_SIZE);
    ui->pushButton_grip_L->setFixedHeight(GRIPPER_SIZE);
    ui->pushButton_grip_R->setFixedWidth(GRIPPER_SIZE);
    ui->pushButton_grip_R->setFixedHeight(GRIPPER_SIZE);

    // ui->pushButton_grip_L->setStyleSheet(pushButton_gripper_style);
    ui->pushButton_grip_L->setStyleSheet(pushButton_gripper_pressed_style);
    ui->pushButton_grip_L->setCheckable(true);
    ui->pushButton_grip_L->setChecked(true);
    ui->pushButton_grip_L->setText("开");

    // ui->pushButton_grip_R->setStyleSheet(pushButton_gripper_style);
    ui->pushButton_grip_R->setStyleSheet(pushButton_gripper_pressed_style);
    ui->pushButton_grip_R->setCheckable(true);
    ui->pushButton_grip_R->setChecked(true);
    ui->pushButton_grip_R->setText("开");

    ui->label_gripper->setAlignment(Qt::AlignCenter);
    ui->label_gripper->setStyleSheet("font-family: Microsoft YaHei; font:bold; font-size: 25px; color: black;");
}

void MainWindow::manualChasisSet()
{
    // ui->pushButton_ch_F->setFixedWidth(80);
    // ui->pushButton_ch_F->setFixedHeight(60);
    // ui->pushButton_ch_L->setFixedWidth(60);
    // ui->pushButton_ch_L->setFixedHeight(80);
    // ui->pushButton_ch_B->setFixedWidth(80);
    // ui->pushButton_ch_B->setFixedHeight(60);
    // ui->pushButton_ch_R->setFixedWidth(60);
    // ui->pushButton_ch_R->setFixedHeight(80);
    ui->pushButton_ch_F->setFixedWidth(CHASIS_MOVE_SIZE2);
    ui->pushButton_ch_F->setFixedHeight(CHASIS_MOVE_SIZE1);
    ui->pushButton_ch_L->setFixedWidth(CHASIS_MOVE_SIZE1);
    ui->pushButton_ch_L->setFixedHeight(CHASIS_MOVE_SIZE2);
    ui->pushButton_ch_B->setFixedWidth(CHASIS_MOVE_SIZE2);
    ui->pushButton_ch_B->setFixedHeight(CHASIS_MOVE_SIZE1);
    ui->pushButton_ch_R->setFixedWidth(CHASIS_MOVE_SIZE1);
    ui->pushButton_ch_R->setFixedHeight(CHASIS_MOVE_SIZE2);

    robotEnPushbutton->setText("机器人使能");
    // robotEnPushbutton->setFixedWidth(130);
    // robotEnPushbutton->setFixedHeight(60);
    robotEnPushbutton->setFixedWidth(EN_BUTTON_WIDTH);
    robotEnPushbutton->setFixedHeight(EN_BUTTON_HEIGHT);
    robotEnPushbutton->setCheckable(true);
    robotEnPushbutton->setStyleSheet(pushButton_en_style);

    QPixmap ch_up_pixmap(":/pngs/icons_ch_up_64.png");
    // 按1.5倍比例放大，保持原始宽高比
    QPixmap upScaledPixmap = ch_up_pixmap.scaled(
        ch_up_pixmap.width() * 1.5,
        ch_up_pixmap.height() * 1.5,
        Qt::KeepAspectRatio,         // 保持宽高比
        Qt::SmoothTransformation     // 平滑缩放，提高质量
    );

    QIcon ch_up_icon(upScaledPixmap);
    ui->pushButton_ch_F->setIcon(ch_up_icon);
    ui->pushButton_ch_F->setIconSize(upScaledPixmap.size());
    ui->pushButton_ch_F->setStyleSheet(pushButton_ch_style);

    QPixmap ch_down_pixmap(":/pngs/icons_ch_down_64.png");
    QPixmap downScaledPixmap = ch_down_pixmap.scaled(
        ch_down_pixmap.width() * 1.5,
        ch_down_pixmap.height() * 1.5,
        Qt::KeepAspectRatio,         // 保持宽高比
        Qt::SmoothTransformation     // 平滑缩放，提高质量
    );
    QIcon ch_down_icon(downScaledPixmap);
    ui->pushButton_ch_B->setIcon(ch_down_icon);
    ui->pushButton_ch_B->setIconSize(downScaledPixmap.size());
    ui->pushButton_ch_B->setLayoutDirection(Qt::LeftToRight);
    ui->pushButton_ch_B->setStyleSheet(pushButton_ch_style);

    QPixmap ch_l_pixmap(":/pngs/icons_ch_l_64.png");
    QPixmap leftScaledPixmap = ch_l_pixmap.scaled(
        ch_l_pixmap.width() * 1.5,
        ch_l_pixmap.height() * 1.5,
        Qt::KeepAspectRatio,         // 保持宽高比
        Qt::SmoothTransformation     // 平滑缩放，提高质量
    );
    QIcon ch_l_icon(leftScaledPixmap);
    ui->pushButton_ch_L->setIcon(ch_l_icon);
    ui->pushButton_ch_L->setIconSize(leftScaledPixmap.size());
    ui->pushButton_ch_L->setLayoutDirection(Qt::LeftToRight);
    ui->pushButton_ch_L->setStyleSheet(pushButton_ch_style);

    QPixmap ch_r_pixmap(":/pngs/icons_ch_r_64.png");
    QPixmap rightScaledPixmap = ch_r_pixmap.scaled(
        ch_r_pixmap.width() * 1.5,
        ch_r_pixmap.height() * 1.5,
        Qt::KeepAspectRatio,         // 保持宽高比
        Qt::SmoothTransformation     // 平滑缩放，提高质量
    );
    QIcon ch_r_icon(rightScaledPixmap);
    ui->pushButton_ch_R->setIcon(ch_r_icon);
    ui->pushButton_ch_R->setIconSize(rightScaledPixmap.size());
    ui->pushButton_ch_R->setLayoutDirection(Qt::LeftToRight);
    ui->pushButton_ch_R->setStyleSheet(pushButton_ch_style);

    ui->label_ch->setAlignment(Qt::AlignCenter);
    ui->label_ch->setStyleSheet("font-family: Microsoft YaHei; font:bold; font-size: 25px; color: black;");

    // chCtrlLayout->setSpacing(20);
    // chCtrlLayout->setHorizontalSpacing(30);
    // chCtrlLayout->setVerticalSpacing(20);
    chCtrlLayout->addWidget(robotEnPushbutton, 0, 1, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->pushButton_grip_L, 1, 0, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->label_gripper, 1, 1, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->pushButton_grip_R, 1, 2, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->pushButton_ch_L, 1, 4, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->pushButton_ch_F, 0, 5, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->pushButton_ch_B, 2, 5, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->pushButton_ch_R, 1, 6, 1, 1, Qt::AlignCenter);
    chCtrlLayout->addWidget(ui->label_ch, 1, 5, 1, 1, Qt::AlignCenter);
    // 在第2列和第3列之间插入间隔项（索引2）
    int specialSpacing = 50;  // 自定义间距值（像素）
    chCtrlLayout->addItem(new QSpacerItem(specialSpacing, 0), 0, 3);
    chCtrlLayout->setSpacing(20);
    chCtrlWidget->setLayout(chCtrlLayout);
}

void MainWindow::manualArmSet()
{
    ui->label_arm->setAlignment(Qt::AlignCenter);
    ui->label_arm->setStyleSheet("font-family: Microsoft YaHei; font:bold; font-size: 25px; color: black;");
    
    leftArmRadioButton->setText("左臂");
    leftArmRadioButton->setStyleSheet(radioButton_style);
    // leftArmRadioButton->setFixedHeight(50);
    // leftArmRadioButton->setFixedWidth(85);
    leftArmRadioButton->setFixedHeight(BUTTON_SELECT_HEIGHT);
    leftArmRadioButton->setFixedWidth(BUTTON_SELECT_WIDTH);
    rightArmRadioButton->setText("右臂");
    rightArmRadioButton->setStyleSheet(radioButton_style);
    // rightArmRadioButton->setFixedHeight(50);
    // rightArmRadioButton->setFixedWidth(85);
    rightArmRadioButton->setFixedHeight(BUTTON_SELECT_HEIGHT);
    rightArmRadioButton->setFixedWidth(BUTTON_SELECT_WIDTH);
    leftArmRadioButton->setChecked(true);

    pushButton_X_plus->setText("X+");
    pushButton_X_plus->setStyleSheet(button_XYZ_style);
    pushButton_X_plus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_X_plus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);
    pushButton_X_minus->setText("X-");
    pushButton_X_minus->setStyleSheet(button_XYZ_style);
    pushButton_X_minus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_X_minus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);

    pushButton_Y_plus->setText("Y+");
    pushButton_Y_plus->setStyleSheet(button_XYZ_style);
    pushButton_Y_plus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Y_plus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);
    pushButton_Y_minus->setText("Y-");
    pushButton_Y_minus->setStyleSheet(button_XYZ_style);
    pushButton_Y_minus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Y_minus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);

    pushButton_Z_plus->setText("Z+");
    pushButton_Z_plus->setStyleSheet(button_XYZ_style);
    pushButton_Z_plus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Z_plus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);
    pushButton_Z_minus->setText("Z-");
    pushButton_Z_minus->setStyleSheet(button_XYZ_style);
    pushButton_Z_minus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Z_minus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);

    pushButton_Rx_plus->setText("Rx+");
    pushButton_Rx_plus->setStyleSheet(button_RXYZ_style);
    pushButton_Rx_plus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Rx_plus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);
    pushButton_Rx_minus->setText("Rx-");
    pushButton_Rx_minus->setStyleSheet(button_RXYZ_style);
    pushButton_Rx_minus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Rx_minus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);

    pushButton_Ry_plus->setText("Ry+");
    pushButton_Ry_plus->setStyleSheet(button_RXYZ_style);
    pushButton_Ry_plus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Ry_plus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);        
    pushButton_Ry_minus->setText("Ry-");
    pushButton_Ry_minus->setStyleSheet(button_RXYZ_style);
    pushButton_Ry_minus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Ry_minus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT); 

    pushButton_Rz_plus->setText("Rz+");
    pushButton_Rz_plus->setStyleSheet(button_RXYZ_style);
    pushButton_Rz_plus->setFixedWidth(PUSHBUTTON_ARM_WIDTH);
    pushButton_Rz_plus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);
    pushButton_Rz_minus->setText("Rz-");
    pushButton_Rz_minus->setStyleSheet(button_RXYZ_style);
    pushButton_Rz_minus->setFixedWidth(PUSHBUTTON_ARM_WIDTH); 
    pushButton_Rz_minus->setFixedHeight(PUSHBUTTON_ARM_HEIGHT);

    armCtrlLayout->setHorizontalSpacing(40);
    armCtrlLayout->setVerticalSpacing(30);
    armCtrlLayout->addWidget(leftArmRadioButton, 0, 0, 1, 2, Qt::AlignCenter);
    armCtrlLayout->addWidget(rightArmRadioButton, 0, 4, 1, 2, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_X_plus, 1, 0, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_X_minus, 1, 1, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Y_plus, 1, 2, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Y_minus, 1, 3, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Z_plus, 1, 4, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Z_minus, 1, 5, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Rx_plus, 2, 0, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Rx_minus, 2, 1, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Ry_plus, 2, 2, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Ry_minus, 2, 3, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Rz_plus, 2, 4, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(pushButton_Rz_minus, 2, 5, 1, 1, Qt::AlignCenter);
    armCtrlLayout->addWidget(ui->label_arm, 0, 2, 1, 2, Qt::AlignCenter);
    armCtrlWidget->setLayout(armCtrlLayout);
}

void MainWindow::manualArmJointsInfoSet()
{
    
    label_joint->setText("关节角");
    label_joint->setAlignment(Qt::AlignCenter);
    label_joint->setStyleSheet(label_joint_style);
 
    label_joint_L->setAlignment(Qt::AlignCenter);
    label_joint_L->setText("左臂关节角");
    label_joint_L->setStyleSheet(label_joint_style);

    label_joint_R->setAlignment(Qt::AlignCenter);
    label_joint_R->setText("右臂关节角");
    label_joint_R->setStyleSheet(label_joint_style);

    label_L_arm_J1->setText("J1(°)");
    label_L_arm_J1->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_J1->setFixedWidth(LABEL_JOINT_WIDTH);
    label_L_arm_J1->setStyleSheet(label_joint_info_style);

    label_L_arm_J2->setText("J2(°)");
    label_L_arm_J2->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_J2->setFixedWidth(LABEL_JOINT_WIDTH);
    label_L_arm_J2->setStyleSheet(label_joint_info_style);
    
    label_L_arm_J3->setText("J3(°)");
    label_L_arm_J3->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_J3->setFixedWidth(LABEL_JOINT_WIDTH);
    label_L_arm_J3->setStyleSheet(label_joint_info_style);
    
    label_L_arm_J4->setText("J4(°)");
    label_L_arm_J4->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_J4->setFixedWidth(LABEL_JOINT_WIDTH);
    label_L_arm_J4->setStyleSheet(label_joint_info_style);
    
    label_L_arm_J5->setText("J5(°)");
    label_L_arm_J5->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_J5->setFixedWidth(LABEL_JOINT_WIDTH);
    label_L_arm_J5->setStyleSheet(label_joint_info_style);
    
    label_L_arm_J6->setText("J6(°)");
    label_L_arm_J6->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_J6->setFixedWidth(LABEL_JOINT_WIDTH);
    label_L_arm_J6->setStyleSheet(label_joint_info_style);
    
    label_L_arm_J7->setText("J7(°)");
    label_L_arm_J7->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_J7->setFixedWidth(LABEL_JOINT_WIDTH);
    label_L_arm_J7->setStyleSheet(label_joint_info_style);

    lineEdit_L_arm_J1->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_J1->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_J1->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_J2->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_J2->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_J2->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_J3->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_J3->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_J3->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_J4->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_J4->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_J4->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_J5->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_J5->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_J5->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_J6->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_J6->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_J6->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_J7->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_J7->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_J7->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    L_arm_J1_layout->addWidget(label_L_arm_J1);
    L_arm_J1_layout->addWidget(lineEdit_L_arm_J1);
    L_arm_J1_layout->setSpacing(0); 
    L_arm_J1_widget->setLayout(L_arm_J1_layout);

    L_arm_J2_layout->addWidget(label_L_arm_J2);
    L_arm_J2_layout->addWidget(lineEdit_L_arm_J2);
    L_arm_J2_layout->setSpacing(0);
    L_arm_J2_widget->setLayout(L_arm_J2_layout);

    L_arm_J3_layout->addWidget(label_L_arm_J3);
    L_arm_J3_layout->addWidget(lineEdit_L_arm_J3);
    L_arm_J3_layout->setSpacing(0);
    L_arm_J3_widget->setLayout(L_arm_J3_layout);

    L_arm_J4_layout->addWidget(label_L_arm_J4);
    L_arm_J4_layout->addWidget(lineEdit_L_arm_J4);
    L_arm_J4_layout->setSpacing(0);
    L_arm_J4_widget->setLayout(L_arm_J4_layout);

    L_arm_J5_layout->addWidget(label_L_arm_J5);
    L_arm_J5_layout->addWidget(lineEdit_L_arm_J5);
    L_arm_J5_layout->setSpacing(0);
    L_arm_J5_widget->setLayout(L_arm_J5_layout);

    L_arm_J6_layout->addWidget(label_L_arm_J6);
    L_arm_J6_layout->addWidget(lineEdit_L_arm_J6);
    L_arm_J6_layout->setSpacing(0);
    L_arm_J6_widget->setLayout(L_arm_J6_layout);

    L_arm_J7_layout->addWidget(label_L_arm_J7);
    L_arm_J7_layout->addWidget(lineEdit_L_arm_J7);
    L_arm_J7_layout->setSpacing(0);
    L_arm_J7_widget->setLayout(L_arm_J7_layout);

    label_R_arm_J1->setText("J1(°)");
    label_R_arm_J1->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_J1->setFixedWidth(LABEL_JOINT_WIDTH);
    label_R_arm_J1->setStyleSheet(label_joint_info_style);
    
    label_R_arm_J2->setText("J2(°)");
    label_R_arm_J2->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_J2->setFixedWidth(LABEL_JOINT_WIDTH);
    label_R_arm_J2->setStyleSheet(label_joint_info_style);
    
    label_R_arm_J3->setText("J3(°)");
    label_R_arm_J3->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_J3->setFixedWidth(LABEL_JOINT_WIDTH);
    label_R_arm_J3->setStyleSheet(label_joint_info_style);
    
    label_R_arm_J4->setText("J4(°)");
    label_R_arm_J4->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_J4->setFixedWidth(LABEL_JOINT_WIDTH);
    label_R_arm_J4->setStyleSheet(label_joint_info_style);
    
    label_R_arm_J5->setText("J5(°)");
    label_R_arm_J5->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_J5->setFixedWidth(LABEL_JOINT_WIDTH);
    label_R_arm_J5->setStyleSheet(label_joint_info_style);
    
    label_R_arm_J6->setText("J6(°)");
    label_R_arm_J6->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_J6->setFixedWidth(LABEL_JOINT_WIDTH);
    label_R_arm_J6->setStyleSheet(label_joint_info_style);
    
    label_R_arm_J7->setText("J7(°)");
    label_R_arm_J7->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_J7->setFixedWidth(LABEL_JOINT_WIDTH);
    label_R_arm_J7->setStyleSheet(label_joint_info_style);
 
    lineEdit_R_arm_J1->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_J1->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_J1->setFixedWidth(LINEEDIT_JOINT_WIDTH);
 
    lineEdit_R_arm_J2->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_J2->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_J2->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_R_arm_J3->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_J3->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_J3->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_R_arm_J4->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_J4->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_J4->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_R_arm_J5->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_J5->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_J5->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_R_arm_J6->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_J6->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_J6->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_R_arm_J7->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_J7->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_J7->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    R_arm_J1_layout->addWidget(label_R_arm_J1);
    R_arm_J1_layout->addWidget(lineEdit_R_arm_J1);
    R_arm_J1_layout->setSpacing(0);    
    R_arm_J1_widget->setLayout(R_arm_J1_layout);

    R_arm_J2_layout->addWidget(label_R_arm_J2);
    R_arm_J2_layout->addWidget(lineEdit_R_arm_J2);
    R_arm_J2_layout->setSpacing(0);    
    R_arm_J2_widget->setLayout(R_arm_J2_layout);

    R_arm_J3_layout->addWidget(label_R_arm_J3);
    R_arm_J3_layout->addWidget(lineEdit_R_arm_J3);
    R_arm_J3_layout->setSpacing(0);    
    R_arm_J3_widget->setLayout(R_arm_J3_layout);

    R_arm_J4_layout->addWidget(label_R_arm_J4);
    R_arm_J4_layout->addWidget(lineEdit_R_arm_J4);
    R_arm_J4_layout->setSpacing(0);    
    R_arm_J4_widget->setLayout(R_arm_J4_layout);

    R_arm_J5_layout->addWidget(label_R_arm_J5);
    R_arm_J5_layout->addWidget(lineEdit_R_arm_J5);
    R_arm_J5_layout->setSpacing(0);    
    R_arm_J5_widget->setLayout(R_arm_J5_layout);

    R_arm_J6_layout->addWidget(label_R_arm_J6);
    R_arm_J6_layout->addWidget(lineEdit_R_arm_J6);
    R_arm_J6_layout->setSpacing(0);    
    R_arm_J6_widget->setLayout(R_arm_J6_layout);

    R_arm_J7_layout->addWidget(label_R_arm_J7);
    R_arm_J7_layout->addWidget(lineEdit_R_arm_J7);
    R_arm_J7_layout->setSpacing(0);    
    R_arm_J7_widget->setLayout(R_arm_J7_layout);

    armJointLayout->addWidget(label_joint_L, 0, 0, 1, 4, Qt::AlignCenter);
    armJointLayout->addWidget(label_joint_R, 0, 4, 1, 4, Qt::AlignCenter);
    armJointLayout->addWidget(L_arm_J1_widget, 1, 0, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(L_arm_J2_widget, 2, 0, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(L_arm_J3_widget, 3, 0, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(L_arm_J4_widget, 4, 0, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(L_arm_J5_widget, 1, 2, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(L_arm_J6_widget, 2, 2, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(L_arm_J7_widget, 3, 2, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(R_arm_J1_widget, 1, 4, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(R_arm_J2_widget, 2, 4, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(R_arm_J3_widget, 3, 4, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(R_arm_J4_widget, 4, 4, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(R_arm_J5_widget, 1, 6, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(R_arm_J6_widget, 2, 6, 1, 2, Qt::AlignCenter);
    armJointLayout->addWidget(R_arm_J7_widget, 3, 6, 1, 2, Qt::AlignCenter);
    armJointWidget->setLayout(armJointLayout);
    armJointWidget->setObjectName("armJointWidget");
    armJointWidget->setStyleSheet("#armJointWidget { background-color: white; border: 5px solid black; border-radius: 5px; }");
}

void MainWindow::manualArmPosInfoSet()
{
    label_coord_L->setText("左臂笛卡尔");
    label_coord_L->setStyleSheet(label_joint_style);
    label_coord_R->setText("右臂笛卡尔");
    label_coord_R->setStyleSheet(label_joint_style);
  
    label_L_arm_X->setText("X");
    label_L_arm_X->setAlignment(Qt::AlignCenter);
    label_L_arm_X->setStyleSheet(label_joint_info_style);
    label_L_arm_X->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_X->setFixedWidth(LABEL_JOINT_WIDTH);

    label_L_arm_Y->setText("Y");
    label_L_arm_Y->setAlignment(Qt::AlignCenter);
    label_L_arm_Y->setStyleSheet(label_joint_info_style);
    label_L_arm_Y->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_Y->setFixedWidth(LABEL_JOINT_WIDTH);
    
    label_L_arm_Z->setText("Z");
    label_L_arm_Z->setAlignment(Qt::AlignCenter);
    label_L_arm_Z->setStyleSheet(label_joint_info_style);
    label_L_arm_Z->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_Z->setFixedWidth(LABEL_JOINT_WIDTH);
    
    label_L_arm_Rx->setText("Rx");
    label_L_arm_Rx->setAlignment(Qt::AlignCenter);
    label_L_arm_Rx->setStyleSheet(label_joint_info_style);
    label_L_arm_Rx->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_Rx->setFixedWidth(LABEL_JOINT_WIDTH);
    
    label_L_arm_Ry->setText("Ry");
    label_L_arm_Ry->setAlignment(Qt::AlignCenter);
    label_L_arm_Ry->setStyleSheet(label_joint_info_style);
    label_L_arm_Ry->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_Ry->setFixedWidth(LABEL_JOINT_WIDTH);
    
    label_L_arm_Rz->setText("Rz");
    label_L_arm_Rz->setAlignment(Qt::AlignCenter);
    label_L_arm_Rz->setStyleSheet(label_joint_info_style);
    label_L_arm_Rz->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_L_arm_Rz->setFixedWidth(LABEL_JOINT_WIDTH);

    lineEdit_L_arm_X->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_X->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_X->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_Y->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_Y->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_Y->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_Z->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_Z->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_Z->setFixedWidth(LINEEDIT_JOINT_WIDTH);
 
    lineEdit_L_arm_Rx->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_Rx->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_Rx->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_Ry->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_Ry->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_Ry->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    lineEdit_L_arm_Rz->setStyleSheet(lineEdit_L_arm_style);
    lineEdit_L_arm_Rz->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_L_arm_Rz->setFixedWidth(LINEEDIT_JOINT_WIDTH);

    L_arm_X_layout->addWidget(label_L_arm_X);
    L_arm_X_layout->addWidget(lineEdit_L_arm_X);
    L_arm_X_layout->setSpacing(0);    
    L_arm_X_widget->setLayout(L_arm_X_layout);

    L_arm_Y_layout->addWidget(label_L_arm_Y);
    L_arm_Y_layout->addWidget(lineEdit_L_arm_Y);
    L_arm_Y_layout->setSpacing(0); 
    L_arm_Y_widget->setLayout(L_arm_Y_layout);

    L_arm_Z_layout->addWidget(label_L_arm_Z);
    L_arm_Z_layout->addWidget(lineEdit_L_arm_Z);
    L_arm_Z_layout->setSpacing(0);
    L_arm_Z_widget->setLayout(L_arm_Z_layout);
    
    L_arm_Rx_layout->addWidget(label_L_arm_Rx);
    L_arm_Rx_layout->addWidget(lineEdit_L_arm_Rx);
    L_arm_Rx_layout->setSpacing(0);
    L_arm_Rx_widget->setLayout(L_arm_Rx_layout);

    L_arm_Ry_layout->addWidget(label_L_arm_Ry);
    L_arm_Ry_layout->addWidget(lineEdit_L_arm_Ry);
    L_arm_Ry_layout->setSpacing(0);
    L_arm_Ry_widget->setLayout(L_arm_Ry_layout);

    L_arm_Rz_layout->addWidget(label_L_arm_Rz);
    L_arm_Rz_layout->addWidget(lineEdit_L_arm_Rz);
    L_arm_Rz_layout->setSpacing(0);    
    L_arm_Rz_widget->setLayout(L_arm_Rz_layout);

    label_R_arm_X->setText("X");
    label_R_arm_X->setAlignment(Qt::AlignCenter);
    label_R_arm_X->setStyleSheet(label_joint_info_style);
    label_R_arm_X->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_X->setFixedWidth(LABEL_JOINT_WIDTH);
        
    label_R_arm_Y->setText("Y");
    label_R_arm_Y->setAlignment(Qt::AlignCenter);
    label_R_arm_Y->setStyleSheet(label_joint_info_style);
    label_R_arm_Y->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_Y->setFixedWidth(LABEL_JOINT_WIDTH);

    label_R_arm_Z->setText("Z");
    label_R_arm_Z->setAlignment(Qt::AlignCenter);
    label_R_arm_Z->setStyleSheet(label_joint_info_style);
    label_R_arm_Z->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_Z->setFixedWidth(LABEL_JOINT_WIDTH);
        
    label_R_arm_Rx->setText("Rx");
    label_R_arm_Rx->setAlignment(Qt::AlignCenter);
    label_R_arm_Rx->setStyleSheet(label_joint_info_style);
    label_R_arm_Rx->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_Rx->setFixedWidth(LABEL_JOINT_WIDTH);
    
    label_R_arm_Ry->setText("Ry");
    label_R_arm_Ry->setAlignment(Qt::AlignCenter);
    label_R_arm_Ry->setStyleSheet(label_joint_info_style);
    label_R_arm_Ry->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_Ry->setFixedWidth(LABEL_JOINT_WIDTH);
    
    label_R_arm_Rz->setText("Rz");
    label_R_arm_Rz->setAlignment(Qt::AlignCenter);
    label_R_arm_Rz->setStyleSheet(label_joint_info_style);
    label_R_arm_Rz->setFixedHeight(LABEL_JOINT_HEIGHT);
    label_R_arm_Rz->setFixedWidth(LABEL_JOINT_WIDTH);

    lineEdit_R_arm_X->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_X->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_X->setFixedWidth(LINEEDIT_JOINT_WIDTH);
    
    lineEdit_R_arm_Y->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_Y->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_Y->setFixedWidth(LINEEDIT_JOINT_WIDTH);
    
    lineEdit_R_arm_Z->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_Z->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_Z->setFixedWidth(LINEEDIT_JOINT_WIDTH);
    
    lineEdit_R_arm_Rx->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_Rx->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_Rx->setFixedWidth(LINEEDIT_JOINT_WIDTH);
    
    lineEdit_R_arm_Ry->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_Ry->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_Ry->setFixedWidth(LINEEDIT_JOINT_WIDTH);
    
    lineEdit_R_arm_Rz->setStyleSheet(lineEdit_R_arm_style);
    lineEdit_R_arm_Rz->setFixedHeight(LINEEDIT_JOINT_HEIGHT);
    lineEdit_R_arm_Rz->setFixedWidth(LINEEDIT_JOINT_WIDTH);
    
    R_arm_X_layout->addWidget(label_R_arm_X);
    R_arm_X_layout->addWidget(lineEdit_R_arm_X);
    R_arm_X_layout->setSpacing(0);    
    R_arm_X_widget->setLayout(R_arm_X_layout);

    R_arm_Y_layout->addWidget(label_R_arm_Y);
    R_arm_Y_layout->addWidget(lineEdit_R_arm_Y);
    R_arm_Y_layout->setSpacing(0);    
    R_arm_Y_widget->setLayout(R_arm_Y_layout);

    R_arm_Z_layout->addWidget(label_R_arm_Z);
    R_arm_Z_layout->addWidget(lineEdit_R_arm_Z);
    R_arm_Z_layout->setSpacing(0);    
    R_arm_Z_widget->setLayout(R_arm_Z_layout);

    R_arm_Rx_layout->addWidget(label_R_arm_Rx);
    R_arm_Rx_layout->addWidget(lineEdit_R_arm_Rx);
    R_arm_Rx_layout->setSpacing(0);    
    R_arm_Rx_widget->setLayout(R_arm_Rx_layout);

    R_arm_Ry_layout->addWidget(label_R_arm_Ry);
    R_arm_Ry_layout->addWidget(lineEdit_R_arm_Ry);
    R_arm_Ry_layout->setSpacing(0);    
    R_arm_Ry_widget->setLayout(R_arm_Ry_layout);

    R_arm_Rz_layout->addWidget(label_R_arm_Rz);
    R_arm_Rz_layout->addWidget(lineEdit_R_arm_Rz);
    R_arm_Rz_layout->setSpacing(0);    
    R_arm_Rz_widget->setLayout(R_arm_Rz_layout);

    // armCoordLayout->setHorizontalSpacing(15);
    // armCoordLayout->setVerticalSpacing(10);
    armCoordLayout->setContentsMargins(20, 10, 30, 30);
    armCoordLayout->addWidget(label_coord_L, 0, 0, 1, 4, Qt::AlignCenter);
    armCoordLayout->addWidget(label_coord_R, 0, 4, 1, 4, Qt::AlignCenter);
    armCoordLayout->addWidget(L_arm_X_widget, 1, 0, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(L_arm_Y_widget, 2, 0, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(L_arm_Z_widget, 3, 0, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(L_arm_Rx_widget, 1, 2, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(L_arm_Ry_widget, 2, 2, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(L_arm_Rz_widget, 3, 2, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(R_arm_X_widget, 1, 4, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(R_arm_Y_widget, 2, 4, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(R_arm_Z_widget, 3, 4, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(R_arm_Rx_widget, 1, 6, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(R_arm_Ry_widget, 2, 6, 1, 2, Qt::AlignCenter);
    armCoordLayout->addWidget(R_arm_Rz_widget, 3, 6, 1, 2, Qt::AlignCenter);
    armCoordWidget->setLayout(armCoordLayout);
    armCoordWidget->setObjectName("armCoordWidget");
    armCoordWidget->setStyleSheet("#armCoordWidget { background-color: white; border: 5px solid black; border-radius: 5px; }");

    armHBoxLayout->setSpacing(20);
    armHBoxLayout->addWidget(armJointWidget, 0);
    armHBoxLayout->addWidget(armCoordWidget, 1);
    armHBoxWidget->setLayout(armHBoxLayout);
}

void MainWindow::manualConnectSlots()
{
    connect(leftArmRadioButton, &QRadioButton::toggled, this, &MainWindow::left_arm_selected);
    connect(rightArmRadioButton, &QRadioButton::toggled, this, &MainWindow::right_arm_selected);

    connect(pushButton_X_plus, &QPushButton::clicked, this, &MainWindow::pushButton_X_plus_clicked);
    connect(pushButton_X_minus, &QPushButton::clicked, this, &MainWindow::pushButton_X_minus_clicked);
 
    connect(pushButton_Y_plus, &QPushButton::clicked, this, &MainWindow::pushButton_Y_plus_clicked);
    connect(pushButton_Y_minus, &QPushButton::clicked, this, &MainWindow::pushButton_Y_minus_clicked);

    connect(pushButton_Z_plus, &QPushButton::clicked, this, &MainWindow::pushButton_Z_plus_clicked);
    connect(pushButton_Z_minus, &QPushButton::clicked, this, &MainWindow::pushButton_Z_minus_clicked);

    connect(pushButton_Rx_plus, &QPushButton::clicked, this, &MainWindow::pushButton_Rx_plus_clicked);
    connect(pushButton_Rx_minus, &QPushButton::clicked, this, &MainWindow::pushButton_Rx_minus_clicked);

    connect(pushButton_Ry_plus, &QPushButton::clicked, this, &MainWindow::pushButton_Ry_plus_clicked);
    connect(pushButton_Ry_minus, &QPushButton::clicked, this, &MainWindow::pushButton_Ry_minus_clicked);

    connect(pushButton_Rz_plus, &QPushButton::clicked, this, &MainWindow::pushButton_Rz_plus_clicked);
    connect(pushButton_Rz_minus, &QPushButton::clicked, this, &MainWindow::pushButton_Rz_minus_clicked);

    connect(robotEnPushbutton, &QPushButton::toggled, this, &MainWindow::robotEn_pressed);

    connect(longPressTimer, &QTimer::timeout, this, &MainWindow::onLongPressTimeout);
}

void MainWindow::manualModeLayoutSet()
{
    mainLayout->removeWidget(ctrlBtnsWidget);
    mainLayout->removeWidget(taskStatusWidget);
    mainLayout->removeWidget(animationWidget);

    mainLayout->addWidget(posDisplayWidget, 0, 2, 2, 3, Qt::AlignCenter);
    mainLayout->addWidget(armHBoxWidget, 2, 0, 3, 6, Qt::AlignCenter);
    mainLayout->addWidget(chCtrlWidget, 5, 0, 4, 3, Qt::AlignCenter);
    mainLayout->addWidget(armCtrlWidget, 5, 3, 4, 3, Qt::AlignCenter);
    mainLayout->setContentsMargins(50,20,50,10);
    mainLayout->update();
}

void MainWindow::autoSwitchToManual()
{
    autoModePageHide();
    manualModePageShow();
    manualModeLayoutSet();
}

void MainWindow::manualSwitchToAuto()
{
    manualModePageHide();
    autoModePageShow();
    autoModeLayoutSet();
}

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    guiToSysBridgeSig = new CommSignal(this);

    widgetsInit();

    autoModeStatusSet();
    
    autoModeCtrlBtnsSet();

    autoModeTaskSet();

    autoModeAnimationSet();

    manualModePosSet();

    manualGripperSet();

    manualChasisSet();

    manualArmSet();

    manualArmJointsInfoSet();

    manualArmPosInfoSet();
    
    manualConnectSlots();

    manualModePageHide();

    autoModeLayoutSet();
}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::printLayoutInfo()
{
    // 获取QGridLayout的行数与列数
    int rowCount = autoModeLayout->rowCount();
    int columnCount = autoModeLayout->columnCount();

    // 遍历每个格子并获取大小
    for(int row = 0; row < rowCount; row ++)
    {
        for(int col = 0; col < columnCount; col ++)
        {
            int rowHeight = autoModeLayout->rowMinimumHeight(row);
            int columnWidth = autoModeLayout->columnMinimumWidth(col);
            qDebug("Row:%d ,Column:%d , Height:%d, Width:%d\n", row, col, rowHeight, columnWidth);
        }
    }
}

void MainWindow::processSysBridgeSig(int sysSig)
{
    // qDebug() << "Received sig from commWithSysBridge.";
    switch(sysSig)
    {
    case ROS_SIG_STEP1_COMPLETED:
        qDebug() << "Received sig is ROS_SIG_STEP1_COMPLETED.";
        this->animationWidget->processStep1();
    break;
    case ROS_SIG_STEP2_COMPLETED:
        qDebug() << "Received sig is ROS_SIG_STEP2_COMPLETED.";
        this->animationWidget->processStep2();
    break;
    case ROS_SIG_STEP3_COMPLETED:
        qDebug() << "Received sig is ROS_SIG_STEP3_COMPLETED.";
        this->animationWidget->processStep3();
    break;
    case ROS_SIG_STEP4_COMPLETED:
        qDebug() << "Received sig is ROS_SIG_STEP4_COMPLETED.";
        this->animationWidget->processStep4();
    break;
    case ROS_SIG_STEP5_COMPLETED:
        qDebug() << "Received sig is ROS_SIG_STEP5_COMPLETED."; 
        this->animationWidget->processStep5();
    break;
    case ROS_SIG_STEP6_COMPLETED:
        qDebug() << "Received sig is ROS_SIG_STEP6_COMPLETED.";
        this->animationWidget->processStep6();
    break;
    case ROS_SIG_SWITCHED_AUTO:
        qDebug() << "Received sig is ROS_SIG_SWITCHED_AUTO.";
        manualSwitchToAuto();
        isAutoMode = true;
    break;
    case ROS_SIG_SWITCHED_MANUAL:
        qDebug() << "Received sig is ROS_SIG_SWITCHED_MANUAL.";
        autoSwitchToManual();
        isAutoMode = false;
    break;
    case ROS_SIG_AUTO_START:
        qDebug() << "Received sig is ROS_SIG_AUTO_START.";
        ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_start_blue_100.png"));
    break;
    case ROS_SIG_AUTO_STOP:
        qDebug() << "Received sig is ROS_SIG_AUTO_STOP.";
        ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_stop_red_100.png"));
    break;
    case ROS_SIG_AUTO_PAUSE:
        qDebug() << "Received sig is ROS_SIG_AUTO_PAUSE.";
        ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_pause_orange_100.png"));
    break;
    case ROS_SIG_STATUS_UPDATE:
        {
            // qDebug() << "Received sig is ROS_SIG_STATUS_UPDATE.";
            ui->lineEdit_pos_X->setText(QString::number(commBridge->robotStatusInfo.chasis_info.pose[0], 'f', 3));
            ui->lineEdit_pos_Y->setText(QString::number(commBridge->robotStatusInfo.chasis_info.pose[1], 'f', 3));
            lineEdit_L_arm_J1->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_left[0], 'f', 3));
            lineEdit_L_arm_J2->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_left[1], 'f', 3));
            lineEdit_L_arm_J3->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_left[2], 'f', 3));
            lineEdit_L_arm_J4->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_left[3], 'f', 3));
            lineEdit_L_arm_J5->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_left[4], 'f', 3));
            lineEdit_L_arm_J6->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_left[5], 'f', 3));
            lineEdit_L_arm_J7->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_left[6], 'f', 3));
            
            lineEdit_R_arm_J1->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_right[0], 'f', 3));
            lineEdit_R_arm_J2->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_right[1], 'f', 3));
            lineEdit_R_arm_J3->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_right[2], 'f', 3));
            lineEdit_R_arm_J4->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_right[3], 'f', 3));
            lineEdit_R_arm_J5->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_right[4], 'f', 3));
            lineEdit_R_arm_J6->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_right[5], 'f', 3));
            lineEdit_R_arm_J7->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_joints_right[6], 'f', 3));
        
            lineEdit_L_arm_X->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_left[0], 'f', 3));
            lineEdit_L_arm_Y->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_left[1], 'f', 3));
            lineEdit_L_arm_Z->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_left[2], 'f', 3));
            lineEdit_L_arm_Rx->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_left[3], 'f', 3));
            lineEdit_L_arm_Ry->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_left[4], 'f', 3));
            lineEdit_L_arm_Rz->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_left[5], 'f', 3));

            lineEdit_R_arm_X->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_right[0], 'f', 3));
            lineEdit_R_arm_Y->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_right[1], 'f', 3));
            lineEdit_R_arm_Z->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_right[2], 'f', 3));
            lineEdit_R_arm_Rx->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_right[3], 'f', 3));
            lineEdit_R_arm_Ry->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_right[4], 'f', 3));
            lineEdit_R_arm_Rz->setText(QString::number(commBridge->robotStatusInfo.arm_info.arm_pose_right[5], 'f', 3));
        }
    break;
    default:
        break;
    }
}

void MainWindow::commSigProcessInit(commWithBridge *commWithBridgeInit)
{
    qDebug() << "commSigProcessInit is called.";
    if(commWithBridgeInit == nullptr)
    {
        qDebug() << "In mainWindow,commWithBridgeInit is nullptr.";
        return ;
    }

    if(commWithBridgeInit->commRosToGuiSig == nullptr)
    {
        qDebug() << "In mainWindow,commWithBridgeInit->commRosToGuiSig is nullptr.";
        return;
    }
    this->commBridge = commWithBridgeInit;
    connect(this->commBridge->commRosToGuiSig, SIGNAL(commSignal(int)), this, SLOT(processSysBridgeSig(int)));
}

void MainWindow::on_pushButton_start_clicked()
{
    qDebug() << "Start button is clicked.";
    // ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_start_blue_100.png"));
    guiToSysBridgeSig->send(GUI_SIG_START);
}

void MainWindow::on_pushButton_stop_clicked()
{
    qDebug() << "Stop button is clicked.";
    // ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_stop_red_100.png"));
    guiToSysBridgeSig->send(GUI_SIG_STOP);
}

void MainWindow::on_pushButton_pause_clicked()
{
    qDebug() << "Pause button is clicked.";
    // ui->label_statusIcon->setPixmap(QPixmap(":/pngs/icons_pause_orange_100.png"));
    guiToSysBridgeSig->send(GUI_SIG_PAUSE);
}

void MainWindow::setLabelStepGreen(int stepNum)
{
    switch(stepNum)
    {
    case 1:
        ui->label_step1->setStyleSheet(label_step_style_green);
        break;
    case 2:
        ui->label_step2->setStyleSheet(label_step_style_green);
        break;
    case 3:
        ui->label_step3->setStyleSheet(label_step_style_green);
        break;
    default:
        break;
    }
}

void MainWindow::setLabelStepGrey(int stepNum)
{
    switch(stepNum)
    {
    case 1:
        ui->label_step1->setStyleSheet(label_step_style);
        break;
    case 2:
        ui->label_step2->setStyleSheet(label_step_style);
        break;
    case 3:
        ui->label_step3->setStyleSheet(label_step_style);
        break;
    default:
        break;
    }
}

void MainWindow::setLabelArrowGreen(int arrowNum)
{
    if(arrowNum == 1)
    {
        ui->label_arrow1->setPixmap(QPixmap(":/pngs/icons_arrow_green_90.png"));
    }
    else if(arrowNum == 2)
    {
        ui->label_arrow2->setPixmap(QPixmap(":/pngs/icons_arrow_green_90.png"));
    }
}

void MainWindow::setLabelArrowGrey(int arrowNum)
{
    if(arrowNum == 1)
    {
        ui->label_arrow1->setPixmap(QPixmap(":/pngs/icons_arrow_grey_90.png"));
    }
    else if(arrowNum == 2)
    {
        ui->label_arrow2->setPixmap(QPixmap(":/pngs/icons_arrow_grey_90.png"));
    }
}

void MainWindow::keyPressEvent(QKeyEvent *event)
{
    switch(event->key())
    {
    case Qt::Key_1:
        qDebug() << "Key_1 is pressed,send TEST_SIG_STEP1.";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_STEP1);
    break;
    case Qt::Key_2:
        qDebug() << "Key_2 is pressed,send TEST_SIG_STEP2";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_STEP2);
    break;
    case Qt::Key_3:
        qDebug() << "Key_3 is pressed,send TEST_SIG_STEP3";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_STEP3);
    break;
    case Qt::Key_4:
        qDebug() << "Key_4 is pressed,send TEST_SIG_STEP4";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_STEP4);
    break;
    case Qt::Key_5:
        qDebug() << "Key_5 is pressed,send TEST_SIG_STEP5";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_STEP5);
    break;
    case Qt::Key_6:
        qDebug() << "Key_6 is pressed,send TEST_SIG_STEP6";
    // 重置动画相关状态变量
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_STEP6);
    break;
    case Qt::Key_7:
        qDebug() << "Key_7 is pressed,send TEST_SIG_AUTO_TO_MANUAL.";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_AUTO_TO_MANUAL);
    break;
    case Qt::Key_8:
        qDebug() << "Key_8 is pressed,send TEST_SIG_MANUAL_TO_AUTO.";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_MANUAL_TO_AUTO);
    break;
    case Qt::Key_Q:
        qDebug() << "Key_q is pressed,send TEST_SIG_AUTO_START.";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_AUTO_START);
    break;
    case Qt::Key_W:
        qDebug() << "Key_q is pressed,send TEST_SIG_AUTO_STOP.";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_AUTO_STOP);
    break;
    case Qt::Key_E:
        qDebug() << "Key_e is pressed,send TEST_SIG_AUTO_STOP.";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_AUTO_PAUSE);
    break;
    case Qt::Key_A:
        qDebug() << "Key_e is pressed,send TEST_SIG_STATUS_UPDATE.";
        guiToSysBridgeSig->send(GUI_ROS_TEST_SIG_STATUS_UPDATE);
    break;
    default:
        qDebug() << "Invalid key is pressed.";
    break;
    }
}

void MainWindow::on_pushButton_auto_clicked()
{
    qDebug() << "Auto button is clicked.";
    if(isAutoMode == true)
    {
        qDebug() << "isAutoMode is true,change to false.";
        // isAutoMode = false;
        guiToSysBridgeSig->send(GUI_SIG_AUTO_TO_MANUAL);
        // autoSwitchToManual();
    }
    else
    {
        qDebug() << "isAutoMode is false,change to true.";
        // isAutoMode = true;
        guiToSysBridgeSig->send(GUI_SIG_MANUAL_TO_AUTO);
        // manualSwitchToAuto();
    }
}

void MainWindow::on_pushButton_ch_F_pressed()
{
    // qDebug() << "pushButton_ch_F is pressed.";
    longPressTimer->start(200);
    guiToSysBridgeSig->send(GUI_SIG_MOVE_F);
}

void MainWindow::on_pushButton_ch_F_released()
{
    qDebug() << "pushButton_ch_F is released.";
    longPressTimer->stop();
}

void MainWindow::on_pushButton_ch_B_pressed()
{
    qDebug() << "pushButton_ch_B is pressed.";
    longPressTimer->start(200);
    guiToSysBridgeSig->send(GUI_SIG_MOVE_B);
}

void MainWindow::on_pushButton_ch_B_released()
{
    qDebug() << "pushButton_ch_B is released.";
    longPressTimer->stop();
}

void MainWindow::on_pushButton_ch_L_pressed()
{
    qDebug() << "pushButton_ch_L is pressed.";
    longPressTimer->start(200);
    guiToSysBridgeSig->send(GUI_SIG_MOVE_L);
}

void MainWindow::on_pushButton_ch_L_released()
{
    qDebug() << "pushButton_ch_L is released.";
    longPressTimer->stop();
}

void MainWindow::on_pushButton_ch_R_pressed()
{
    qDebug() << "pushButton_ch_R is pressed.";
    longPressTimer->start(200);
    guiToSysBridgeSig->send(GUI_SIG_MOVE_R);
}

void MainWindow::on_pushButton_ch_R_released()
{
    qDebug() << "pushButton_ch_R is released.";
    longPressTimer->stop();
}

void MainWindow::onLongPressTimeout()
{
    // qDebug() << "Long press check timeout.";
    longPressTimer->start(200);
    if(ui->pushButton_ch_F->isDown())
    {
        guiToSysBridgeSig->send(GUI_SIG_MOVE_F);
    }
    else if(ui->pushButton_ch_B->isDown())
    {
        guiToSysBridgeSig->send(GUI_SIG_MOVE_B);
    }
    else if(ui->pushButton_ch_L->isDown())
    {
        guiToSysBridgeSig->send(GUI_SIG_MOVE_L);
    }
    else if(ui->pushButton_ch_R->isDown())
    {
        guiToSysBridgeSig->send(GUI_SIG_MOVE_R);
    }
}

void MainWindow::on_pushButton_grip_L_toggled(bool checked)
{
    if(checked == true)
    {
        qDebug() << "Left gripper is pressed, open.";
        ui->pushButton_grip_L->setStyleSheet(pushButton_gripper_pressed_style);
        ui->pushButton_grip_L->setText("开");
        guiToSysBridgeSig->send(GUI_SIG_GRIPPER_OPEN_L);
    }
    else
    {
        qDebug() << "Left gripper is released, close.";
        ui->pushButton_grip_L->setStyleSheet(pushButton_gripper_style);
        ui->pushButton_grip_L->setText("关");
        guiToSysBridgeSig->send(GUI_SIG_GRIPPER_CLOSE_L);
    }
}


void MainWindow::on_pushButton_grip_R_toggled(bool checked)
{
    if(checked == true)
    {
        qDebug() << "Right gripper is pressed, open.";
        ui->pushButton_grip_R->setStyleSheet(pushButton_gripper_pressed_style);
        ui->pushButton_grip_R->setText("开");
        guiToSysBridgeSig->send(GUI_SIG_GRIPPER_OPEN_R);
    }
    else
    {
        qDebug() << "Right gripper is released, close.";
        ui->pushButton_grip_R->setStyleSheet(pushButton_gripper_style);
        ui->pushButton_grip_R->setText("关");
        guiToSysBridgeSig->send(GUI_SIG_GRIPPER_CLOSE_R);
    }
}

void MainWindow::robotEn_pressed(bool checked)
{
    if(checked)
    {
        qDebug() << "robot is enabled";
        guiToSysBridgeSig->send(GUI_SIG_ROBOT_EN);
    }
    else
    {
        qDebug() << "robot is disabled";
        guiToSysBridgeSig->send(GUI_SIG_ROBOT_DIS);
    }
}

void MainWindow::left_arm_selected(bool checked)
{
    if(checked)
    {
        qDebug() << "left arm is selected";
        isLeftArmSelected = true;
    }
    else
    {
        qDebug() << "left arm is unselected";
        isLeftArmSelected = false;
    }
}

void MainWindow::right_arm_selected(bool checked)
{
    if(checked)
    {
        qDebug() << "right arm is selected"; 
        isLeftArmSelected = false;
    }
    else
    {
        qDebug() << "right arm is unselected";
        isLeftArmSelected = true;
    }
}

void MainWindow::pushButton_X_plus_clicked()
{
    qDebug() << "pushButton_X_plus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_XI_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_XI_R);
    }
}

void MainWindow::pushButton_X_minus_clicked()
{
    qDebug() << "pushButton_X_minus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_XD_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_XD_R);
    }
}

void MainWindow::pushButton_Y_plus_clicked()
{
    qDebug() << "pushButton_Y_plus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_YI_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_YI_R);
    }
}

void MainWindow::pushButton_Y_minus_clicked()
{
    qDebug() << "pushButton_Y_minus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_YD_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_YD_R);
    }
}

void MainWindow::pushButton_Z_plus_clicked()
{
    qDebug() << "pushButton_Z_plus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_ZI_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_ZI_R);
    }
}

void MainWindow::pushButton_Z_minus_clicked()
{
    qDebug() << "pushButton_Z_minus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_ZD_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_ZD_R);
    }
}

void MainWindow::pushButton_Rx_plus_clicked()
{
    qDebug() << "pushButton_Rx_plus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RXI_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RXI_R);
    }
}

void MainWindow::pushButton_Rx_minus_clicked()
{
    qDebug() << "pushButton_Rx_minus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RXD_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RXD_R);
    }
}

void MainWindow::pushButton_Ry_plus_clicked()
{
    qDebug() << "pushButton_Ry_plus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RYI_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RYI_R);
    }
}

void MainWindow::pushButton_Ry_minus_clicked()
{
    qDebug() << "pushButton_Ry_minus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RYD_L);
    } 
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RYD_R);
    }
}

void MainWindow::pushButton_Rz_plus_clicked()
{
    qDebug() << "pushButton_Rz_plus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RZI_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RZI_R);
    }
}

void MainWindow::pushButton_Rz_minus_clicked()
{
    qDebug() << "pushButton_Rz_minus is clicked.";
    if(isLeftArmSelected)
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RZD_L);
    }
    else
    {
        guiToSysBridgeSig->send(GUI_SIG_ARM_RZD_R);
    }
}

